#include "search.h"
#include <vector>
#if(KEIL)
#include "SEEKFREE_MT9V03X.h"
#endif

#include "search.h"


/********************************************变量声明***************************************************/
uint8        g_VideoImageData[MAX_VIDEO_LINE][MAX_VIDEO_POINT];  //图像原始数据，g_VideoImageData[m][n]代表图像的从上数第m行，从左数第n点，0为最黑，255为最白
//==============================图像数据================================//
_POINT  g_CenterPosition[MAX_POINT_NUM];
_POINT  g_LeftEdge[MAX_POINT_NUM], g_RightEdge[MAX_POINT_NUM];
int          g_CenterNum;
int          g_LeftEdgeNum, g_RightEdgeNum;
//==============================图像处理用变量==========================//
uint16       g_Start[10], g_End[10], g_SEnum;
uint8        g_SearchFlag = 1;
int          g_Cover;
uint8        g_CoverIndex[10], g_CoverNum;
//==============================控制用变量==============================//
int			g_DirectionControl = MID_VIDEO_POINT;
int			g_FormerDirectionControl = MID_VIDEO_POINT;
int			g_DirectionControlWhole = 0;
int			g_DirectionControlLine = 0;
//======================================================================//


//========================================新增变量定义====================================//
int IMAGE_MIDDLE = 130;
uint8 bin_image[image_h][image_w];  // 二值化图像数组
uint8 start_point_l[2] = { 0 };//左边起点的x，y值
uint8 start_point_r[2] = { 0 };//右边起点的x，y值
//存放点的x，y坐标
_POINT points_l[(uint16)USE_num];//左线
_POINT points_r[(uint16)USE_num];//右线
uint16 dir_r[(uint16)USE_num] = { 0 };//用来存储右边生长方向
uint16 dir_l[(uint16)USE_num] = { 0 };//用来存储左边生长方向
uint16 data_stastics_l = 0;//统计左边找到点的个数
uint16 data_stastics_r = 0;//统计右边找到点的个数
uint8 hightest = 0;//最高点
uint8 l_border[image_h];//左线数组
uint8 r_border[image_h];//右线数组
uint8 center_line[image_h];//中线数组
//====================================================================================//

int16 limit_a_b(int16 x, int a, int b)
{
    if (x < a) x = a;
    if (x > b) x = b;
    return x;
}

/*最小二乘法*/
float SlopeCalculate(uint8 begin, uint8 end, uint8* border)
{
    float xsum = 0, ysum = 0, xysum = 0, x2sum = 0;
    int16 i = 0;
    float result = 0;
    static float resultlast;

    for (i = begin; i < end; i++)
    {
        xsum += i;
        ysum += border[i];
        xysum += i * (border[i]);
        x2sum += i * i;

    }
    if ((end - begin) * x2sum - xsum * xsum) //判断除数是否为零
    {
        result = ((end - begin) * xysum - xsum * ysum) / ((end - begin) * x2sum - xsum * xsum);
        resultlast = result;
    }
    else
    {
        result = resultlast;
    }
    return result;
}

/*计算斜率截距*/
void calculate_s_i(uint8 start, uint8 end, uint8* border, float* slope_rate, float* intercept)
{
    uint16 i, num = 0;
    uint16 xsum = 0, ysum = 0;
    float y_average, x_average;

    num = 0;
    xsum = 0;
    ysum = 0;
    y_average = 0;
    x_average = 0;
    for (i = start; i < end; i++)
    {
        xsum += i;
        ysum += border[i];
        num++;
    }

    //计算各个平均数
    if (num)
    {
        x_average = (float)(xsum / num);
        y_average = (float)(ysum / num);

    }

    //  计算斜率
    *slope_rate = SlopeCalculate(start, end, border);//斜率
    *intercept = y_average - (*slope_rate) * x_average;//截距
}

/*十字补线函数*/
void cross_fill(uint8(*image)[image_w], uint8* l_border, uint8* r_border, uint16 total_num_l, uint16 total_num_r,
    uint16* dir_l, uint16* dir_r, uint16(*points_l)[2], uint16(*points_r)[2])
{
    uint8 i;
    uint8 break_num_l = 0;
    uint8 break_num_r = 0;
    uint8 start, end;
    float slope_l_rate = 0, intercept_l = 0;
    //出十字
    for (i = 1; i < total_num_l; i++)
    {
        if (dir_l[i - 1] == 4 && dir_l[i] == 4 && dir_l[i + 3] == 6 && dir_l[i + 5] == 6 && dir_l[i + 7] == 6)
        {
            break_num_l = points_l[i][1];//传递y坐标
            printf("brea_knum-L:%d\n", break_num_l);
            printf("I:%d\n", i);
            printf("十字标志位：1\n");
            break;
        }
    }
    for (i = 1; i < total_num_r; i++)
    {
        if (dir_r[i - 1] == 4 && dir_r[i] == 4 && dir_r[i + 3] == 6 && dir_r[i + 5] == 6 && dir_r[i + 7] == 6)
        {
            break_num_r = points_r[i][1];//传递y坐标
            printf("brea_knum-R:%d\n", break_num_r);
            printf("I:%d\n", i);
            printf("十字标志位：1\n");
            break;
        }
    }
    if (break_num_l && break_num_r && image[image_h - 1][4] && image[image_h - 1][image_w - 4])//两边生长方向都符合条件
    {
        //计算斜率
        start = break_num_l - 15;
        start = limit_a_b(start, 0, image_h);
        end = break_num_l - 5;
        calculate_s_i(start, end, l_border, &slope_l_rate, &intercept_l);
        for (i = break_num_l - 5; i < image_h - 1; i++)
        {
            l_border[i] = slope_l_rate * (i)+intercept_l;//y = kx+b
            l_border[i] = limit_a_b(l_border[i], border_min, border_max);//限幅
        }

        //计算斜率
        start = break_num_r - 15;//起点
        start = limit_a_b(start, 0, image_h);//限幅
        end = break_num_r - 5;//终点
        calculate_s_i(start, end, r_border, &slope_l_rate, &intercept_l);
        for (i = break_num_r - 5; i < image_h - 1; i++)
        {
            r_border[i] = slope_l_rate * (i)+intercept_l;
            r_border[i] = limit_a_b(r_border[i], border_min, border_max);
        }
    }
}

// 大津法动态阈值
void otsuThreshold()
{
    // 大津法
    unsigned long Amount = 0;
    unsigned long PixelBack = 0;
    unsigned long PixelshortegralBack = 0;
    unsigned long Pixelshortegral = 0;
    signed long PixelshortegralFore = 0;
    signed long PixelFore = 0;
    float OmegaBack, OmegaFore, MicroBack, MicroFore, SigmaB, Sigma; // 类间方差
    signed short MinValue = 0, MaxValue = 255;
    unsigned char HistoGram[256] = { 0 };
    for (int i = 0; i < MAX_VIDEO_LINE; i++)
        for (int j = 0; j < MAX_VIDEO_POINT; j++)
            HistoGram[g_VideoImageData[i][j]]++;
    for (; MinValue < 256 && HistoGram[MinValue] == 0; MinValue++);
    for (; MaxValue > MinValue && HistoGram[MaxValue] != 0; MaxValue--);
    if (MaxValue == MinValue || (MinValue + 1) == MaxValue)
    {
        IMAGE_MIDDLE = MinValue;
    }
    else
    {
        Pixelshortegral = 0;
        SigmaB = -1;
        for (int i = MinValue; i <= MaxValue; i++)
        {
            Amount += HistoGram[i];  // 像素点总数
            Pixelshortegral += HistoGram[i] * i;  // 灰度值总数
        }
        float tmp = Pixelshortegral;
        float average_pixel = tmp / Amount;
        for (int i = MinValue; i <= MaxValue; i++)
        {
            PixelBack = PixelBack + HistoGram[i];  // 前景像素点数
            PixelFore = Amount - PixelBack;  // 背景像素点数
            OmegaBack = ((float)PixelBack) / Amount;  // 前景像素百分比
            OmegaFore = ((float)PixelFore) / Amount;  // 背景像素百分比
            PixelshortegralBack += HistoGram[i] * i; // 前景灰度值
            PixelshortegralFore = Pixelshortegral - PixelshortegralBack;  // 背景灰度值
            MicroBack = ((float)PixelshortegralBack) / PixelBack;  // 前景灰度百分比
            MicroFore = ((float)PixelshortegralFore) / PixelFore;  // 背景灰度百分比
            Sigma = PixelBack * (MicroBack - average_pixel) * (MicroBack - average_pixel) + PixelFore * (MicroFore - average_pixel) * (MicroFore - average_pixel);  // 类间方差
            if (Sigma > SigmaB)
            {
                SigmaB = Sigma;
                IMAGE_MIDDLE = i;
            }
        }
    }
    printf("当前图片帧对应的相对最佳阈值:%d\n", IMAGE_MIDDLE);
}

// 二值化
void turn_to_bin(void)
{
    for (int i = 0; i < MAX_VIDEO_LINE; i++)
        for (int j = 0; j < MAX_VIDEO_POINT; j++)
            if (g_VideoImageData[i][j] == 255)
                g_VideoImageData[i][j] = 230;
    otsuThreshold();
    for (int i = 0; i < image_h; i++)
    {
        for (int j = 0; j < image_w; j++)
        {
            if (g_VideoImageData[i][j] > IMAGE_MIDDLE)
                bin_image[i][j] = white_pixel;
            else
                bin_image[i][j] = black_pixel;
        }
    }
}


/*寻找两个边界的边界点作为八邻域循环的起始点*/
uint8 get_start_point(uint8 start_row)
{
    uint8 i = 0, l_found = 0, r_found = 0;
    //清零
    start_point_l[0] = 0;//x
    start_point_l[1] = 0;//y

    start_point_r[0] = 0;//x
    start_point_r[1] = 0;//y

    //从中间往左边，先找起点
    for (i = image_w / 2; i > border_min; i--)
    {
        start_point_l[0] = i;//x
        start_point_l[1] = start_row;//y
        if (bin_image[start_row][i] == 255 && bin_image[start_row][i - 1] == 0)
        {
            //printf("找到左边起点image[%d][%d]\n", start_row,i);
            l_found = 1;
            break;
        }
    }

    for (i = image_w / 2; i < border_max; i++)
    {
        start_point_r[0] = i;//x
        start_point_r[1] = start_row;//y
        if (bin_image[start_row][i] == 255 && bin_image[start_row][i + 1] == 0)
        {
            //printf("找到右边起点image[%d][%d]\n",start_row, i);
            r_found = 1;
            break;
        }
    }

    if (l_found && r_found)return 1;
    else {
        //printf("未找到起点\n");
        return 0;
    }
}

/*八邻域正式开始找右边点的函数，左右线一次性找完。 */
void search_l_r(uint16 break_flag, uint8(*image)[image_w], uint16* l_stastic, uint16* r_stastic, uint8 l_start_x, uint8 l_start_y, uint8 r_start_x, uint8 r_start_y, uint8* hightest)
{
    uint8 i = 0, j = 0;

    //左边变量
    uint8 search_filds_l[8][2] = { {  0 } };
    uint8 index_l = 0;
    uint8 temp_l[8][2] = { {  0 } };
    uint8 center_point_l[2] = { 0 };
    uint16 l_data_statics;//统计左边
    //定义八个邻域
    static int8 seeds_l[8][2] = { {0,  1},{-1,1},{-1,0},{-1,-1},{0,-1},{1,-1},{1,  0},{1, 1}, };
    //{-1,-1},{0,-1},{+1,-1},
    //{-1, 0},       {+1, 0},
    //{-1,+1},{0,+1},{+1,+1},
    //这个是顺时针

    //右边变量
    uint8 search_filds_r[8][2] = { {  0 } };
    uint8 center_point_r[2] = { 0 };//中心坐标点
    uint8 index_r = 0;//索引下标
    uint8 temp_r[8][2] = { {  0 } };
    uint16 r_data_statics;//统计右边
    //定义八个邻域
    static int8 seeds_r[8][2] = { {0,  1},{1,1},{1,0}, {1,-1},{0,-1},{-1,-1}, {-1,  0},{-1, 1}, };
    //{-1,-1},{0,-1},{+1,-1},
    //{-1, 0},       {+1, 0},
    //{-1,+1},{0,+1},{+1,+1},
    //这个是逆时针

    l_data_statics = *l_stastic;//统计找到了多少个点，方便后续把点全部画出来
    r_data_statics = *r_stastic;//统计找到了多少个点，方便后续把点全部画出来

    //第一次更新坐标点  将找到的起点值传进来
    center_point_l[0] = l_start_x;//x
    center_point_l[1] = l_start_y;//y
    center_point_r[0] = r_start_x;//x
    center_point_r[1] = r_start_y;//y

    //开启邻域循环
    while (break_flag--)
    {
        //左边
        for (i = 0; i < 8; i++)//传递8F坐标
        {
            search_filds_l[i][0] = center_point_l[0] + seeds_l[i][0];//x
            search_filds_l[i][1] = center_point_l[1] + seeds_l[i][1];//y
        }
        //中心坐标点填充到已经找到的点内
        points_l[l_data_statics++].x = center_point_l[0];//x
        points_l[l_data_statics++].y = center_point_l[1];//y
        // l_data_statics++;//索引加一

        //右边
        for (i = 0; i < 8; i++)//传递8F坐标
        {
            search_filds_r[i][0] = center_point_r[0] + seeds_r[i][0];//x
            search_filds_r[i][1] = center_point_r[1] + seeds_r[i][1];//y
        }
        //中心坐标点填充到已经找到的点内
        points_r[r_data_statics].x = center_point_r[0];//x
        points_r[r_data_statics].y = center_point_r[1];//y

        index_l = 0;//先清零，后使用
        for (i = 0; i < 8; i++)
        {
            temp_l[i][0] = 0;//先清零，后使用
            temp_l[i][1] = 0;//先清零，后使用
        }

        //左边判断
        for (i = 0; i < 8; i++)
        {
            if (image[search_filds_l[i][1]][search_filds_l[i][0]] == 0
                && image[search_filds_l[(i + 1) & 7][1]][search_filds_l[(i + 1) & 7][0]] == 255)
            {
                temp_l[index_l][0] = search_filds_l[(i)][0];
                temp_l[index_l][1] = search_filds_l[(i)][1];
                index_l++;
                dir_l[l_data_statics - 1] = (i);//记录生长方向
            }

            if (index_l)
            {
                //更新坐标点
                center_point_l[0] = temp_l[0][0];//x
                center_point_l[1] = temp_l[0][1];//y
                for (j = 0; j < index_l; j++)
                {
                    if (center_point_l[1] > temp_l[j][1])
                    {
                        center_point_l[0] = temp_l[j][0];//x
                        center_point_l[1] = temp_l[j][1];//y
                    }
                }
            }
        }
        if ((points_r[r_data_statics].x == points_r[r_data_statics - 1].x && points_r[r_data_statics].x == points_r[r_data_statics - 2].x
            && points_r[r_data_statics].y == points_r[r_data_statics - 1].y && points_r[r_data_statics].y == points_r[r_data_statics - 2].y)
            || (points_l[l_data_statics - 1].x == points_l[l_data_statics - 2].x && points_l[l_data_statics - 1].x == points_l[l_data_statics - 3].x
                && points_l[l_data_statics - 1].y == points_l[l_data_statics - 2].y && points_l[l_data_statics - 1].y == points_l[l_data_statics - 3].y))
        {
            break;
        }
        if (ABS(points_r[r_data_statics].x - points_l[l_data_statics - 1].x) < 2
            && ABS(points_r[r_data_statics].y - points_l[l_data_statics - 1].y) < 2)
        {
            *hightest = (points_r[r_data_statics].y + points_l[l_data_statics - 1].y) >> 1;//取出最高点
            break;
        }
        if ((points_r[r_data_statics].y < points_l[l_data_statics - 1].y))
        {
            printf("\n如果左边比右边高了，左边等待右边\n");
            continue;//如果左边比右边高了，左边等待右边
        }
        if (dir_l[l_data_statics - 1] == 7
            && (points_r[r_data_statics].y > points_l[l_data_statics - 1].y))//左边比右边高且已经向下生长了
        {
            center_point_l[0] = points_l[l_data_statics - 1].x;//x
            center_point_l[1] = points_l[l_data_statics - 1].y;//y
            l_data_statics--;
        }
        r_data_statics++;//索引加一

        index_r = 0;//先清零，后使用
        for (i = 0; i < 8; i++)
        {
            temp_r[i][0] = 0;//先清零，后使用
            temp_r[i][1] = 0;//先清零，后使用
        }

        //右边判断
        for (i = 0; i < 8; i++)
        {
            if (image[search_filds_r[i][1]][search_filds_r[i][0]] == 0
                && image[search_filds_r[(i + 1) & 7][1]][search_filds_r[(i + 1) & 7][0]] == 255)
            {
                temp_r[index_r][0] = search_filds_r[(i)][0];
                temp_r[index_r][1] = search_filds_r[(i)][1];
                index_r++;//索引加一
                dir_r[r_data_statics - 1] = (i);//记录生长方向
            }
            if (index_r)
            {
                //更新坐标点
                center_point_r[0] = temp_r[0][0];//x
                center_point_r[1] = temp_r[0][1];//y
                for (j = 0; j < index_r; j++)
                {
                    if (center_point_r[1] > temp_r[j][1])
                    {
                        center_point_r[0] = temp_r[j][0];//x
                        center_point_r[1] = temp_r[j][1];//y
                    }
                }
            }
        }
    }
    //取出循环次数
    *l_stastic = l_data_statics;
    *r_stastic = r_data_statics;
}

/*从八邻域边界里提取需要的边线*/
void get_left(uint16 total_L)
{
    uint8 i = 0;
    uint16 j = 0;
    uint8 h = 0;
    //初始化
    for (i = 0; i < image_h; i++)
    {
        l_border[i] = border_min;
    }
    h = image_h - 2;
    //左边
    for (j = 0; j < total_L; j++)
    {
        //printf("%d\n", j);
        if (points_l[j].y == h)
        {
            l_border[h] = points_l[j].x + 1;
        }
        else 
            continue; //每行只取一个点，没到下一行就不记录
        h--;
        if (h == 0)
        {
            break;//到最后一行退出
        }
    }
}

/*从八邻域边界里提取需要的边线*/
void get_right(uint16 total_R)
{
    uint8 i = 0;
    uint16 j = 0;
    uint8 h = 0;
    for (i = 0; i < image_h; i++)
    {
        r_border[i] = border_max;//右边线初始化放到最右边，左边线放到最左边，这样八邻域闭合区域外的中线就会在中间，不会干扰得到的数据
    }
    h = image_h - 2;
    //右边
    for (j = 0; j < total_R; j++)
    {
        if (points_r[j].y == h)
        {
            r_border[h] = points_r[j].x - 1;
        }
        else continue;//每行只取一个点，没到下一行就不记录
        h--;
        if (h == 0)break;//到最后一行退出
    }
}

/*膨胀和腐蚀*/
void image_filter(uint8(*bin_image)[image_w])
{
    uint16 i, j;
    uint32 num = 0;
    for (i = 1; i < image_h - 1; i++)
    {
        for (j = 1; j < (image_w - 1); j++)
        {
            //统计八个方向的像素值
            num = bin_image[i - 1][j - 1] + bin_image[i - 1][j] + bin_image[i - 1][j + 1] + bin_image[i][j - 1] + bin_image[i][j + 1] 
                + bin_image[i + 1][j - 1] + bin_image[i + 1][j] + bin_image[i + 1][j + 1];
            if (num >= threshold_max && bin_image[i][j] == 0)
            {
                bin_image[i][j] = 255;//白  可以搞成宏定义，方便更改
            }
            if (num <= threshold_min && bin_image[i][j] == 255)
            {
                bin_image[i][j] = 0;//黑
            }
        }
    }
}

/*给图像画一个黑框*/
void image_draw_rectan(uint8(*image)[image_w])
{
    uint8 i = 0;
    for (i = 0; i < image_h; i++)
    {
        image[i][0] = 0;
        image[i][1] = 0;
        image[i][image_w - 1] = 0;
        image[i][image_w - 2] = 0;

    }
    for (i = 0; i < image_w; i++)
    {
        image[0][i] = 0;
        image[1][i] = 0;
    }
}

/*最终处理函数 */
void Search(void)
{
    uint16 i;
    uint8 hightest = 0;//定义一个最高行，即y值的最小
    /*这是离线调试用的*/
    turn_to_bin();
    /*提取赛道边界*/
    image_filter(bin_image);//滤波
    image_draw_rectan(bin_image);//预处理
    //清零
    data_stastics_l = 0;
    data_stastics_r = 0;
    if (get_start_point(image_h - 2))//找到起点了，再执行八领域，没找到就一直找
    {
        printf("正在开始八领域\n");
        search_l_r((uint16)USE_num, bin_image, &data_stastics_l, &data_stastics_r, start_point_l[0], start_point_l[1], start_point_r[0], start_point_r[1], &hightest);
        printf("八邻域已结束\n");
        // 从爬取的边界线内提取边线
        get_left(data_stastics_l);
        get_right(data_stastics_r);
        //处理函数放这里，不要放到if外面去了，不要放到if外面去了，不要放到if外面去了，重要的事说三遍
    }

    for (i = hightest; i < image_h - 1; i++)
    {
        center_line[i] = (l_border[i] + r_border[i]) >> 1;//求中线
    }
}
